// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/five_point_relative_pose.h"

#include <Eigen/Dense>
#include <glog/logging.h>

#include <cmath>
#include <ctime>
#include <vector>

#include "theia/math/polynomial.h"
#include "theia/sfm/pose/util.h"

namespace theia {

using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::RowVector3d;
using Eigen::RowVector4d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;
using Eigen::VectorXd;

typedef Matrix<double, 10, 10> Matrix10d;

namespace {

// Multiply two degree one polynomials of variables x, y, z.
// E.g. p1 = a[0]x + a[1]y + a[2]z + a[3]
// Output order: x^2 xy y^2 xz yz z^2 x y z 1 (GrevLex)
Matrix<double, 1, 10> MultiplyDegOnePoly(const RowVector4d& a,
                                         const RowVector4d& b) {
  Matrix<double, 1, 10> output;
  // x^2
  output(0) = a(0) * b(0);
  // xy
  output(1) = a(0) * b(1) + a(1) * b(0);
  // y^2
  output(2) = a(1) * b(1);
  // xz
  output(3) = a(0) * b(2) + a(2) * b(0);
  // yz
  output(4) = a(1) * b(2) + a(2) * b(1);
  // z^2
  output(5) = a(2) * b(2);
  // x
  output(6) = a(0) * b(3) + a(3) * b(0);
  // y
  output(7) = a(1) * b(3) + a(3) * b(1);
  // z
  output(8) = a(2) * b(3) + a(3) * b(2);
  // 1
  output(9) = a(3) * b(3);
  return output;
}

// Multiply a 2 deg poly (in x, y, z) and a one deg poly in GrevLex order.
// x^3 x^2y xy^2 y^3 x^2z xyz y^2z xz^2 yz^2 z^3 x^2 xy y^2 xz yz z^2 x y z 1
Matrix<double, 1, 20> MultiplyDegTwoDegOnePoly(const Matrix<double, 1, 10>& a,
                                               const RowVector4d& b) {
  Matrix<double, 1, 20> output;
  // x^3
  output(0) = a(0) * b(0);
  // x^2y
  output(1) = a(0) * b(1) + a(1) * b(0);
  // xy^2
  output(2) = a(1) * b(1) + a(2) * b(0);
  // y^3
  output(3) = a(2) * b(1);
  // x^2z
  output(4) = a(0) * b(2) + a(3) * b(0);
  // xyz
  output(5) = a(1) * b(2) + a(3) * b(1) + a(4) * b(0);
  // y^2z
  output(6) = a(2) * b(2) + a(4) * b(1);
  // xz^2
  output(7) = a(3) * b(2) + a(5) * b(0);
  // yz^2
  output(8) = a(4) * b(2) + a(5) * b(1);
  // z^3
  output(9) = a(5) * b(2);
  // x^2
  output(10) = a(0) * b(3) + a(6) * b(0);
  // xy
  output(11) = a(1) * b(3) + a(6) * b(1) + a(7) * b(0);
  // y^2
  output(12) = a(2) * b(3) + a(7) * b(1);
  // xz
  output(13) = a(3) * b(3) + a(6) * b(2) + a(8) * b(0);
  // yz
  output(14) = a(4) * b(3) + a(7) * b(2) + a(8) * b(1);
  // z^2
  output(15) = a(5) * b(3) + a(8) * b(2);
  // x
  output(16) = a(6) * b(3) + a(9) * b(0);
  // y
  output(17) = a(7) * b(3) + a(9) * b(1);
  // z
  output(18) = a(8) * b(3) + a(9) * b(2);
  // 1
  output(19) = a(9) * b(3);
  return output;
}

Matrix<double, 1, 20> GetDeterminantConstraint(
    const Matrix<double, 1, 4> null_space[3][3]) {
  // Singularity constraint.
  const Matrix<double, 1, 20> determinant =
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space[0][1], null_space[1][2]) -
          MultiplyDegOnePoly(null_space[0][2], null_space[1][1]),
          null_space[2][0]) +
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space[0][2], null_space[1][0]) -
          MultiplyDegOnePoly(null_space[0][0], null_space[1][2]),
          null_space[2][1]) +
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space[0][0], null_space[1][1]) -
          MultiplyDegOnePoly(null_space[0][1], null_space[1][0]),
          null_space[2][2]);
  return determinant;
}

// Shorthand for multiplying the Essential matrix with its transpose.
Matrix<double, 1, 10> EETranspose(
    const Matrix<double, 1, 4> null_space[3][3], int i, int j) {
  return MultiplyDegOnePoly(null_space[i][0], null_space[j][0]) +
      MultiplyDegOnePoly(null_space[i][1], null_space[j][1]) +
      MultiplyDegOnePoly(null_space[i][2], null_space[j][2]);
}


// Builds the trace constraint: EEtE - 1/2 trace(EEt)E = 0
Matrix<double, 9, 20> GetTraceConstraint(
    const Matrix<double, 1, 4> null_space[3][3]) {
  Matrix<double, 9, 20> trace_constraint;

  // Comput EEt.
  Matrix<double, 1, 10> eet[3][3];
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      eet[i][j] = 2 * EETranspose(null_space, i, j);
    }
  }

  // Compute the trace.
  const Matrix<double, 1, 10> trace = eet[0][0] + eet[1][1] + eet[2][2];

  // Multiply EEt with E.
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      trace_constraint.row(3 * i + j) =
          MultiplyDegTwoDegOnePoly(eet[i][0], null_space[0][j]) +
          MultiplyDegTwoDegOnePoly(eet[i][1], null_space[1][j]) +
          MultiplyDegTwoDegOnePoly(eet[i][2], null_space[2][j]) -
          0.5 * MultiplyDegTwoDegOnePoly(trace, null_space[i][j]);
    }
  }

  return trace_constraint;
}

Matrix<double, 10, 20> BuildConstraintMatrix(
    const Matrix<double, 1, 4> null_space[3][3]) {
  Matrix<double, 10, 20> constraint_matrix;
  constraint_matrix.block<9, 20>(0, 0) = GetTraceConstraint(null_space);
  constraint_matrix.row(9) = GetDeterminantConstraint(null_space);
  return constraint_matrix;
}

}  // namespace

// Implementation of Nister from "An Efficient Solution to the Five-Point
// Relative Pose Problem"
bool FivePointRelativePose(const std::vector<Vector2d>& image1_points,
                           const std::vector<Vector2d>& image2_points,
                           std::vector<Matrix3d>* essential_matrices) {
  CHECK_EQ(image1_points.size(), image2_points.size());
  CHECK_GE(image1_points.size(), 5) << "You must supply at least 5 "
                                       "correspondences for the 5 point "
                                       "essential matrix algorithm.";

  // Step 1. Create the nx9 matrix containing epipolar constraints.
  //   Essential matrix is a linear combination of the 4 vectors spanning the
  //   null space of this matrix.
  MatrixXd epipolar_constraint(image1_points.size(), 9);
  for (int i = 0; i < image1_points.size(); i++) {
    // Fill matrix with the epipolar constraint from q'_t*E*q = 0. Where q is
    // from the first image, and q' is from the second.
    epipolar_constraint.row(i) <<
        image2_points[i].x() * image1_points[i].x(),
        image2_points[i].y() * image1_points[i].x(),
        image1_points[i].x(),
        image2_points[i].x() * image1_points[i].y(),
        image2_points[i].y() * image1_points[i].y(),
        image1_points[i].y(),
        image2_points[i].x(),
        image2_points[i].y(),
        1.0;
  }

  Matrix<double, 9, 4> null_space;

  // Extract the null space from a minimal sampling (using LU) or non-minimal
  // sampling (using SVD).
  if (image1_points.size() == 5) {
    const Eigen::FullPivLU<MatrixXd> lu(epipolar_constraint);
    if (lu.dimensionOfKernel() != 4) {
      return false;
    }
    null_space = lu.kernel();
  } else {
    const Eigen::JacobiSVD<MatrixXd> svd(
        epipolar_constraint.transpose() * epipolar_constraint,
        Eigen::ComputeFullV);
    null_space = svd.matrixV().rightCols<4>();
  }

  const Matrix<double, 1, 4> null_space_matrix[3][3] = {
    { null_space.row(0), null_space.row(3), null_space.row(6) },
    { null_space.row(1), null_space.row(4), null_space.row(7) },
    { null_space.row(2), null_space.row(5), null_space.row(8) }
  };

  // Step 2. Expansion of the epipolar constraints on the determinant and trace.
  const Matrix<double, 10, 20> constraint_matrix =
      BuildConstraintMatrix(null_space_matrix);

  // Step 3. Eliminate part of the matrix to isolate polynomials in z.
  Eigen::FullPivLU<Matrix10d> c_lu(constraint_matrix.block<10, 10>(0, 0));
  Matrix10d eliminated_matrix =
      c_lu.solve(constraint_matrix.block<10, 10>(0, 10));

  Matrix10d action_matrix = Matrix10d::Zero();
  action_matrix.block<3, 10>(0, 0) = eliminated_matrix.block<3, 10>(0, 0);
  action_matrix.row(3) = eliminated_matrix.row(4);
  action_matrix.row(4) = eliminated_matrix.row(5);
  action_matrix.row(5) = eliminated_matrix.row(7);
  action_matrix(6, 0) = -1.0;
  action_matrix(7, 1) = -1.0;
  action_matrix(8, 3) = -1.0;
  action_matrix(9, 6) = -1.0;

  Eigen::EigenSolver<Matrix10d> eigensolver(action_matrix);
  const auto& eigenvectors = eigensolver.eigenvectors();
  const auto& eigenvalues = eigensolver.eigenvalues();

  // Now that we have x, y, and z we need to substitute them back into the null
  // space to get a valid essential matrix solution.
  for (int i = 0; i < 10; i++) {
    // Only consider real solutions.
    if (eigenvalues(i).imag() != 0) {
      continue;
    }
    Matrix3d ematrix;
    Map<Matrix<double, 9, 1> >(ematrix.data()) =
        null_space * eigenvectors.col(i).tail<4>().real();
    essential_matrices->emplace_back(ematrix);
  }

  return essential_matrices->size() > 0;
}

}  // namespace theia
